﻿# include "utils.h"
//#include "clipper2/clipper.h"


using namespace std;

double norm(Point a) {    //向量的模
	return sqrt(a.x * a.x + a.y * a.y);
}

double norm(const Line& a) {
	Point Sp = a.Pt1;
	Point Ep = a.Pt2;
	return norm(Ep - Sp);
}

double cross(Point a, Point b) {    //叉乘
	return a.x * b.y - a.y * b.x;
}


//计算任意两个线段的最近距离
double getTwoLinesDist(Line line1, Line line2) {
	if (crossSegs(line1, line2)) {
		return 0;
	}

	double DistLeft_1, DistLeft_2, DistRight_1, DistRight_2;
	double linesDistance;
	vector<double> distNums;

	DistLeft_1 = pointToSegmentDistance(line1.Pt1, line2);
	DistLeft_2 = pointToSegmentDistance(line1.Pt2, line2);
	DistRight_1 = pointToSegmentDistance(line2.Pt1, line1);
	DistRight_2 = pointToSegmentDistance(line2.Pt2, line1);

	distNums.emplace_back(DistLeft_1);
	distNums.emplace_back(DistLeft_2);
	distNums.emplace_back(DistRight_1);
	distNums.emplace_back(DistRight_2);

	linesDistance = backMinDistNum(distNums);

	return linesDistance;
}

//从一堆距离集合里面返回最小的一个值(一堆数中选个最小的)
double backMinDistNum(vector<double> DistNums) {
	double minDistance;
	double tempDistance;
	//Init
	minDistance = DistNums[0];
	for (int i = 1; i < DistNums.size(); ++i) {
		tempDistance = DistNums[i];
		if (tempDistance < minDistance) {
			minDistance = tempDistance;
		}
	}

	return minDistance;
}

//计算一个点到一条线段的最短距离
double pointToSegmentDistance(Point basePoint, Line baseLine) {
	bool checkBaseProjection;
	double lessDistance;
	Point baseProjectionPoint, nearestPoint;

	baseProjectionPoint = getProjectionPoint(basePoint, baseLine);
	checkBaseProjection = checkPointInSegment(baseProjectionPoint, baseLine);

	if (checkBaseProjection) {
		lessDistance = linearDist(baseProjectionPoint, basePoint);
	}
	else {
		nearestPoint = getLineNearestPoint(baseProjectionPoint, baseLine);
		lessDistance = linearDist(nearestPoint, basePoint);
	}

	return lessDistance;
}
//求出投影点的位置
Point getProjectionPoint(Point basePoint, Line baseLine) {
	Point baseLineVec, baseLineVecticalVec;

	baseLineVec = baseLine.Pt2 - baseLine.Pt1;
	baseLineVecticalVec = rotate(baseLineVec, 90);

	return getInsection(baseLineVecticalVec, basePoint, baseLineVec, baseLine.Pt1);

}

Point RotateVec(Point vec, double angle) {
	double rad = angle * PI / 180.0;
	Point rotate_vec = { vec.x * cos(rad) - vec.y * sin(rad), vec.y * cos(rad) + vec.x * sin(rad) };
	return rotate_vec;
}

Point getInsection(Point Vec1, Point p1, Point Vec2, Point p2) {
	Point P1P2 = p2 - p1;
	double t = cross(P1P2, Vec2) / cross(Vec1, Vec2);
	Point outPoint = p1 + Vec1 * t; 

	return outPoint;
}
///向量旋转,[1] > 0:逆时针
Point rotate(Point vec, double angle) {
	double rad = angle * PI / 180.0;
	Point rotate_vec = { vec.x * cos(rad) - vec.y * sin(rad), vec.y * cos(rad) + vec.x * sin(rad) };
	return rotate_vec;
}


// 优化判断：两条线段是否有交点  
bool crossSegs(Line line1, Line line2) {
	bool insectPoint;
	bool judge1, judge2;
	Point crossPoint;
	Point vecLine1, vecLine2;
	vecLine1 = line1.Pt2 - line1.Pt1;
	vecLine2 = line2.Pt2 - line2.Pt1;
	NormalizeVec(vecLine1);
	NormalizeVec(vecLine2);
	double t = cross(vecLine1, vecLine2);
	if (abs(t) < CountMinValue) {  // 平行线
		return false;  //无交点
	}

	// if EndingPoint On OtherSegment == Cross  !!! Adding it will result in many points that cannot be calculated and found
	//if (judgeCrossPoint(line2.Pt1, line1) || judgeCrossPoint(line2.Pt2, line1)
	//	|| judgeCrossPoint(line1.Pt1, line2) || judgeCrossPoint(line1.Pt2, line2)) {
	//	return true;
	//}

	crossPoint = getInsection(vecLine1, line1.Pt1, vecLine2, line2.Pt2);  // 交点
	judge1 = judgeCrossPoint(crossPoint, line1);
	judge2 = judgeCrossPoint(crossPoint, line2);
	if (judge1 && judge2) {
		insectPoint = true;
	}
	else {
		insectPoint = false;
	}

	return insectPoint;
}

// 判断点是否在线段中间
bool judgeCrossPoint(Point point, Line line) {

	if (point == line.Pt1 || point == line.Pt2) {
		return true;
	}

	Point sP, eP;
	bool judgeResult = false;
	sP = line.Pt1;
	eP = line.Pt2;

	Point vec1_ = sP - point;  // 2023/11/7 更新新的判断方法
	Point vec2_ = eP - point;
	NormalizeVec(vec1_);
	NormalizeVec(vec2_);
	double t = cross(vec1_, vec2_);
	if (abs(t) > CountMinValue) {  // 不平行
		return false;
	}

	if (pointToLineDist(point, line) > MapMinValue) {
		return false;
	}

	if ((vec1_ * vec2_) < 0) {
		return true;
	}
	else {
		return false;
	}

	//double distL, distR;
	//double segmentLength;
	//distL = linearDist(point, sP);
	//distR = linearDist(point, eP);
	//segmentLength = linearDist(sP, eP);
	//if (abs(segmentLength - (distL + distR)) < MapMinValue) {    // 距离很接近也直接按相等处理
	//	return true;
	//}
	//else {
	//	return false;
	//}

}


double CrossVecs(Point a, Point b) {    //叉乘
	return a.x * b.y - a.y * b.x;
}

void NormalizeVec(Point& NormalVec) {
	double VecLength = sqrt(NormalVec.x * NormalVec.x + NormalVec.y * NormalVec.y);
	if (VecLength != 0 && VecLength != 1) {
		NormalVec = NormalVec / VecLength;
	}
}

//点到直线的距离
double pointToLineDist(Point point, Line line1) {
	Point vecLine;
	Point vecNormalLine;
	Point crossingPoint;
	vecLine = line1.Pt2 - line1.Pt1;
	vecNormalLine = rotate(vecLine, 90);
	crossingPoint = getInsection(vecLine, line1.Pt1, vecNormalLine, point);
	double dist = linearDist(point, crossingPoint);

	return dist;
}


//得到两点之间的直线距离
double linearDist(Point point1, Point point2) {
	double distance;
	distance = sqrt((point1.x - point2.x) * (point1.x - point2.x) + (point1.y - point2.y) * (point1.y - point2.y));
	return distance;
}

//在baseL中找到和baseP最近的端点
Point getLineNearestPoint(Point baseP, Line baseL) {
	double leftDistance, rightDistance;

	leftDistance = rightDistance = DoubleMaxNumValue;
	leftDistance = linearDist(baseP, baseL.Pt1);
	rightDistance = linearDist(baseP, baseL.Pt2);

	if (leftDistance < rightDistance) {
		return baseL.Pt1;
	}
	else {
		return baseL.Pt2;
	}
}



//检查一个点是否在一条线段上面
bool checkPointInSegment(Point tP, Line segmentL) {
	double distL, distR;
	double segmentLength;
	distL = linearDist(tP, segmentL.Pt1);
	distR = linearDist(tP, segmentL.Pt2);
	segmentLength = linearDist(segmentL.Pt1, segmentL.Pt2);
	if (abs(segmentLength - (distL + distR)) < MapMinValue) {  //距离很接近也直接按相等处理
		return true;
	}
	else {
		return false;
	}
}
void exchangePoints(Point& point1, Point& point2) {
	Point tempP;
	tempP = point1;
	point1 = point2;
	point2 = tempP;
}
/*
* @brief 用来判断推挤线是否超过相邻线段的水平位置
* @line 存放推挤线
* @point 相邻线段的固定点
* @return 1： 表明固定点在推挤线的逆时针方向  -1：表明固定点在推挤线的顺时针方向  0：同向
*/
int direction(Line& line, Point& point) {
	double c = cross(line.Pt2 - line.Pt1, point - line.Pt1);
	if (c > 0)
		return 1;        //两向量夹角为正
	else if (c < 0)
		return -1;  //两向量夹角为负
	else
		return 0;
}
// 两条线是否是平行线
bool isParallel_lines(Line line1, Line line2) {
	bool resultJudge;//output
	double t;
	Point vec1, vec2;
	vec1 = line1.Pt2 - line1.Pt1;
	vec2 = line2.Pt2 - line2.Pt1;
	NormalizeVec(vec1);
	NormalizeVec(vec2);
	t = cross(vec1, vec2);
	if (abs(t) < CountMinValue) {
		resultJudge = true;
	}
	else {
		resultJudge = false;
	}
	return resultJudge;
}
bool CheckLinesPerpendicular(Line firstSeg, Line SecondSeg) {
	Point vec1 = firstSeg.Pt2 - firstSeg.Pt1;
	Point vec2 = SecondSeg.Pt2 - SecondSeg.Pt1;
	vec1 = vec1 / norm(vec1);
	vec2 = vec2 / norm(vec2);

	double dotRes = vec1 * vec2;

	if (abs(dotRes) < CountMinValue) {
		return true;
	}
	else {
		return false;
	}
}
bool inspectVectorsDirection(Point vec1, Point vec2) {
	double dotResult;
	NormalizeVec(vec1);  // 20240110 Update
	NormalizeVec(vec2);

	dotResult = vec1 * vec2;  //点积

	if (abs(dotResult) < CountMinValue) {
		dotResult = 0;
	}
	if (dotResult >= 0) {
		return true;
	}
	else {
		return false;
	}
}
void removeLine(Line line, vector<Line>& lines) {
	int sizeVec;
	bool overLoop = true;

	sizeVec = lines.size();

	for (int i = 0; i < sizeVec; ++i) {
		if ((line.Pt1 == lines[i].Pt1 && line.Pt2 == lines[i].Pt2)
			|| (line.Pt1 == lines[i].Pt2 && line.Pt2 == lines[i].Pt1)
			) {
			lines.erase(lines.begin() + i); //
			//cout << "The Line :" << line << "removed!" << endl;
			overLoop = false;
			break;
		}
	}

	if (overLoop) {
		cout << "Error! The Line is not in the lines!" << endl;
	}
}
// 获取当前线段在线组中的位置
int getPosition(const vector<Line>& lines, Line& line) {
	int index = -1;
	for (int i = 0; i < lines.size(); ++i) {    //定位推挤线在总体折线的位置
		if (line.Pt1 == lines[i].Pt1 && line.Pt2 == lines[i].Pt2) {
			index = i;
			line = lines[i];		//（补充）将line更新为lines中目标线段的索引
			break;
		}
	}
	return index;
}
void mergeLines(vector<Line>& lines) {
	//删除长度为0的线段，合并相邻且共线的线段
	for (int i = 0; i + 1 < lines.size(); ++i) {		//确保i+1不出界
		Point veci = lines.at(i).getVector();
		double Lengthi = lines.at(i).getLength();
		Point veci1 = lines.at(i + 1).getVector();
		double Lengthi1 = lines.at(i + 1).getLength();
		if (Lengthi < CountMinValue) {					//线段i长度为0，删除
			if (i == 0)
				lines.at(i + 1).Pt1 = lines.at(i).Pt1;
			else
				lines.at(i - 1).Pt2 = lines.at(i).Pt2;
			lines.erase(lines.begin() + i);
		}
		else if (Lengthi1 < CountMinValue) {			//线段i+1长度为0，删除，MapMinValue
			lines.at(i).Pt2 = lines.at(i + 1).Pt2;
			//维护走线，此处不需要维护lockLines
			lines.erase(lines.begin() + i + 1);
		}
		else if (abs(cross(veci / Lengthi, veci1 / Lengthi1)) < AngleMaxValue
			&& lines.at(i).width == lines.at(i + 1).width) {	//线段i与线段i+1长度均不为0，但共线，并且宽度相等，合并AngleMaxValue

			//维护走线
			lines.at(i).Pt2 = lines.at(i + 1).Pt2;
			lines.erase(lines.begin() + i + 1);
		}
	}
}
bool pointInPolygon(Point p, const std::vector<std::vector<Line>>& obss, bool polygonIncludeOutline) {		//判断点是否在障碍物内部
	bool isInside = false;
	for (int i = 0; i < obss.size(); i++) {
		int crossing_num = 0;
		for (const Line& obs_line : obss[i]) {
			const Point& p1 = obs_line.Pt1;
			const Point& p2 = obs_line.Pt2;
			double min_y = min(p1.y, p2.y);
			double max_y = max(p1.y, p2.y);
			double min_x = min(p1.x, p2.x);
			double max_x = max(p1.x, p2.x);
			if (p.y >= min_y && p.y < max_y) {
				if (max_y - min_y < MapMinValue) {		// obs_line是水平线(接近水平线)
					if (p.x < min_x - MapMinValue)
						++crossing_num;
					else if (p.x < max_x + MapMinValue)
						return polygonIncludeOutline;
					else
						continue;
				}
				else {			// obs_line不是水平线
					double x = (p.y - p1.y) * (p2.x - p1.x) / (p2.y - p1.y) + p1.x;
					if (abs(p.x - x) < MapMinValue)	// 交点与p重合，p在边界上
						return polygonIncludeOutline;
					if (x > p.x)
						++crossing_num;
				}
			}
		}
		if (crossing_num % 2 == 1) {
			isInside = true;
			break;
		}
	}
	return isInside;
}



size_t v2Hash::PointHash::operator()(const Point& pnt) const 
{
	return std::hash<double>()(round(pnt.x * 1e6))^ std::hash<double>()(round(pnt.y * 1e6));
}

size_t v2Hash::LineHash::operator()(const Line& line) const 
{
	return PointHash()(line.Pt1) ^ PointHash()(line.Pt2);
}



// Hug
namespace bg = boost::geometry;
typedef std::vector<Line> LineSet;

bool CheckSegmentAngle(const Line& InputSegment) {
	double Alpha = CountLineAngle(InputSegment);
	if (
		abs(Alpha) <= CountMinValue ||
		abs(Alpha - 45) <= CountMinValue ||
		abs(Alpha - 90) <= CountMinValue ||
		abs(Alpha - 135) <= CountMinValue ||
		abs(Alpha - 180) <= CountMinValue
		) {

		return true;
	}
	else {
		return false;
	}
}

Point CountLineMidPos(const Point& Point1, const Point& Point2) {
	Point MidPoint;
	MidPoint.x = (Point1.x + Point2.x) / 2;
	MidPoint.y = (Point1.y + Point2.y) / 2;

	return MidPoint;
}

Point CountLineMidPos(const Line& BaseSegment) {
	return CountLineMidPos(BaseSegment.Pt1, BaseSegment.Pt2);
}

double CountLineAngle(const Line& BaseSeg) {
	Point StartPoint = BaseSeg.Pt1;
	Point EndPoint = BaseSeg.Pt2;

	Point SegmentVecDir = EndPoint - StartPoint;
	//SegmentVecDir.normalizeVec();
	NormalizeVec(SegmentVecDir);
	Point InferenceVec = Point(1, 0);
	double CosAlphaValue = SegmentVecDir * InferenceVec;
	double Alpha = acos(CosAlphaValue);  // 0 - Pi
	Alpha = Alpha * 180.0 / PI;
	return Alpha;
}


Point CountCrossingPoint(Point Vec1, Point p1, Point Vec2, Point p2) {
	Point P1P2 = p2 - p1;
	double t = CrossVecs(P1P2, Vec2) / CrossVecs(Vec1, Vec2);
	Point outPoint = p1 + Vec1 * t;

	return outPoint;
}


Point getCenterOfObstacles(const vector<vector<Line> >& InputObstacles) {
	Point obsCenterPoint;
	int countNum = 0;
	vector<Point> ItemPointsVec;
	for (auto& ItemObs : InputObstacles) {
		ItemPointsVec = LinesToPoints(ItemObs, true);
		for (int i = 0; i < ItemPointsVec.size(); ++i) {
			obsCenterPoint = obsCenterPoint + ItemPointsVec[i];
			countNum++;
		}
	}

	obsCenterPoint = obsCenterPoint / countNum;

	return obsCenterPoint;

}

// Points[] -> Line
//flag:[1] false(0): 当前为将点->Line; [2] true(1): 需要封闭
vector<Line> PointsToLines(vector<Point> points, bool flag) {
	vector<Line> vecLines;
	if (points.size() <= 1) {   // 2023/11/9 update
		return vecLines;
	}

	//vector<Line> vecLines;
	Point startP, endP;
	Line tempLine;
	for (int i = 0; i < points.size() - 1; ++i) {
		startP = points[i];
		endP = points[i + 1];
		tempLine.setPt1(startP);
		tempLine.setPt2(endP);
		vecLines.emplace_back(tempLine);
	}
	if (flag) {
		endP = points[0];
		startP = points[points.size() - 1];
		tempLine.setPt1(startP);
		tempLine.setPt2(endP);
		vecLines.emplace_back(tempLine);
	}
	return vecLines;
}

/*
* brief: Lines -> points
* @flag: 当前的线是封闭否 flag = true ：封闭的{不考虑最后一条线的终点}
*/
vector<Point> LinesToPoints(vector<Line> lines, bool flag) {
	vector<Point> outPoints;
	//Bug : Exit   // 2023/11/9 update
	if (lines.empty()) {
		return outPoints;
	}

	if (!flag) { //非封闭的线组
		outPoints.emplace_back(lines[0].Pt1);
	}

	for (int i = 0; i < lines.size(); ++i) {
		outPoints.emplace_back(lines[i].Pt2);
		//cout << "Point:" << lines[i].Pt2 << endl;
	}

	return outPoints;
}

void DeleteSameSegment(vector<Line>& InputLinesVec) {
	vector<Line> DeleteSegs;

	for (int i = 0; i + 1 < InputLinesVec.size(); ++i) {
		if (InputLinesVec[i] == InputLinesVec[i + 1]) {  // 相邻线段相同的话就需要都删除掉
			DeleteSegs.emplace_back(InputLinesVec[i]);
			DeleteSegs.emplace_back(InputLinesVec[i + 1]);
			i++;    // 进行到下一根线
		}
	}

	for (auto& itemSeg : DeleteSegs) {
		removeLine(itemSeg, InputLinesVec);
	}
}

bool CheckNumInSets(const double& InputNumber, const vector<double>& InputNumSets) {
	if (InputNumSets.empty()) {
		return false;
	}

	for (double ItemNum : InputNumSets) {
		if (InputNumber == ItemNum) {
			return true;
		}
	}

	return false;

}

bool CheckNumInSets(const int& InputNumber, const vector<int>& InputNumSets) {
	if (InputNumSets.empty()) {
		return false;
	}

	for (double ItemNum : InputNumSets) {
		if (InputNumber == ItemNum) {
			return true;
		}
	}

	return false;

}

vector<vector<Line> > ExpandWire(const vector<Line>& InputWireLines, const double& ExpandDistance) {
	vector<vector<Line> > ExpandWireLines;
	if (InputWireLines.empty()) {
		return ExpandWireLines;
	}
	vector<Line> ExpandSegs;
	for (int i = 0; i < InputWireLines.size(); ++i) {
		ExpandSegs = ExpandSegment(InputWireLines[i], ExpandDistance);
		//logData(ExpandSegs, "E:\\PostgraduateProgram\\VsProgram\\Slide\\SlideTest\\outPut\\LogData\\1226SlidePolyLines.txt", true, false);
		ExpandWireLines.emplace_back(ExpandSegs);
	}

	return ExpandWireLines;
}

vector<Line> ExpandSegment(const Line& InputSegment, const double& ExpandDistance) {
	double PointMoveDist = ExpandDistance * sqrt(2);
	vector<Point> TwoPointsVec;
	TwoPointsVec.emplace_back(InputSegment.Pt1);
	TwoPointsVec.emplace_back(InputSegment.Pt2);
	vector<Point> OutPoints;
	if (TwoPointsVec[0] != TwoPointsVec[1]) {
		Point SegDirVec = TwoPointsVec[1] - TwoPointsVec[0];  // 基准向量
		NormalizeVec(SegDirVec);
		Point MoveDirVec = RotateVec(SegDirVec, 135);
		for (int i = 0; i < TwoPointsVec.size(); ++i) {
			Point BasePoint = TwoPointsVec[i];
			for (int j = 0; j < 2; ++j) {
				Point NewFirstPoint = BasePoint + MoveDirVec * PointMoveDist;
				OutPoints.emplace_back(NewFirstPoint);
				MoveDirVec = RotateVec(MoveDirVec, 90);
			}
		}
	}
	else {  // 线是一个点
		Point MoveDirVec = Point(1, 0);
		MoveDirVec = RotateVec(MoveDirVec, 135);
		for (int i = 0; i < 4; ++i) {
			Point NewFirstPoint = TwoPointsVec[0] + MoveDirVec * PointMoveDist;
			OutPoints.emplace_back(NewFirstPoint);
			MoveDirVec = RotateVec(MoveDirVec, 90);
		}
	}


	vector<Line> ExpandSegLines = PointsToLines(OutPoints, true);

	// 切角处理
	double ChamferLength = (PointMoveDist - ExpandDistance) * sqrt(2);
	for (int i = 0; i < ExpandSegLines.size(); ++i) {
		Line ProcessSeg = ExpandSegLines[i];
		Point ChangeDir = ProcessSeg.Pt2 - ProcessSeg.Pt1;
		NormalizeVec(ChangeDir);
		ExpandSegLines[i].Pt1 = ExpandSegLines[i].Pt1 + ChangeDir * ChamferLength;
		ExpandSegLines[i].Pt2 = ExpandSegLines[i].Pt2 - ChangeDir * ChamferLength;
	}
	vector<Line> CountExpandSegLines = ExpandSegLines;
	for (int i = 0; i < 4; ++i) {
		if (i != 3) {
			Line AddSeg = Line(CountExpandSegLines[i].Pt2, CountExpandSegLines[i + 1].Pt1);
			ExpandSegLines.emplace_back(AddSeg);
		}
		else {
			Line AddSeg = Line(CountExpandSegLines[i].Pt2, CountExpandSegLines[0].Pt1);
			ExpandSegLines.emplace_back(AddSeg);
		}
	}

	sortLines(ExpandSegLines);

	return ExpandSegLines;
}

//对一组线进行首尾相连排列 Pt1 -> Pt2
void sortLines(vector<Line>& lines) {
	if (lines.empty()) {
		return;
	}

	RemovePointSegment(lines);

	Line baseLine;
	Line tempLine;
	Line nextLine;
	int sizeOfLines;
	vector<Line> outLines;
	//初始化
	sizeOfLines = lines.size();
	baseLine = lines[0];   //Pt1 -> Pt2
	lines.erase(lines.begin()); //
	//removeLine(baseLine, vector<Line>&lines)
	outLines.emplace_back(baseLine);

	for (int i = 1; i < sizeOfLines; ++i) {
		nextLine = getSideLine(baseLine, lines);
		removeLine(nextLine, lines);
		baseLine = nextLine;
		outLines.emplace_back(baseLine);
	}

	lines.clear();  //清空
	for (int j = 0; j < outLines.size(); ++j) {
		lines.emplace_back(outLines[j]);
	}
}

//一组线中找到和line相连接的线
Line getSideLine(Line line, vector<Line> lines) {
	Point sidePoint;
	Line outLine;

	sidePoint = line.Pt2;
	outLine = line;

	for (int i = 0; i < lines.size(); ++i) {
		if (sidePoint == lines[i].Pt1) {  //线段的起点和line的终点相连
			outLine = lines[i];
			break;
		}
		if (sidePoint == lines[i].Pt2) {  //线段的终点和line的终点相连
			exchangePoints(lines[i].Pt1, lines[i].Pt2);
			outLine = lines[i];
			break;
		}
	}

	//if (outLine.Pt1 == line.Pt1 && outLine.Pt2 == line.Pt2) {
	//	cout << "Error! There is no line side the 'line'! " << endl;
	//}
	return outLine;
}


// 获取当前点在点集中的位置
int getPointPosition(const vector<Point>& pointsVec, const Point& basePoint) {
	int index = -1;
	//Point tempPoint = basePoint;
	for (int i = 0; i < pointsVec.size(); ++i) {
		if (basePoint == pointsVec[i]) {
			index = i;
			break;
		}
	}

	return index;
}


//判断输入的一组线是障碍物还是线
bool isEncloseGraph(vector<Line> lines) {
	bool isPolygon;
	bool newPoint;
	int linesSize;  //number of lines in vector
	Line tempLine;
	vector<Point> vertexPoint;  //顶点
	//Init
	linesSize = lines.size();

	for (int i = 0; i < lines.size(); ++i) {
		tempLine = lines[i];
		newPoint = pointInPointsVec(tempLine.Pt1, vertexPoint);
		if (!newPoint) {
			vertexPoint.emplace_back(tempLine.Pt1);
		}
		newPoint = pointInPointsVec(tempLine.Pt2, vertexPoint);
		if (!newPoint) {
			vertexPoint.emplace_back(tempLine.Pt2);
		}
	}
	isPolygon = (vertexPoint.size() == linesSize) ? true : false;
	return isPolygon;
}

//判断一个点是否在一个点集中
bool pointInPointsVec(Point point, vector<Point> points) {
	bool outResult = false;
	if (points.empty()) {
		return false;
	}

	for (int i = 0; i < points.size(); ++i) {
		if (point == points[i]) {
			outResult = true;
			break;
		}
	}
	return outResult;
}

//将Points中的点全部添加到newPointsVec中去
void pointsToNewVec(vector<Point> points_, vector<Point>& newPointsVec) {
	//Going!
	for (int i = 0; i < points_.size(); ++i) {
		bool NullPoint = pointInPointsVec(points_[i], newPointsVec);
		if (!NullPoint) {
			newPointsVec.emplace_back(points_[i]);
		}
	}
}


//将两点直接连成线
vector<Line> linkToLine(Point sp_, Point ep_) {
	vector<Line> outPrintLines;
	Line line;
	line.setPt1(sp_);
	line.setPt2(ep_);
	outPrintLines.emplace_back(line);

	return outPrintLines;
}

//  求两点的中点
Point getMidpoint(Point point1, Point point2) {
	Point middlePoint;
	middlePoint.x = (point1.x + point2.x) / 2;
	middlePoint.y = (point1.y + point2.y) / 2;
	return middlePoint;
}

//  求两点的中点
Point getMidpoint(const Line& line) {
	Point middlePoint;
	middlePoint = getMidpoint(line.Pt1, line.Pt2);
	return middlePoint;
}

//去除点集中的某个点
void removePoint(Point point, vector<Point>& points) {
	int num = points.size();  // 12-25 Update
	for (int i = 0; i < points.size(); ++i) {
		if (point == points[i]) { //找到了点集中的点
			points.erase(points.begin() + i); //
			break;
		}
	}
	//if (num == points.size()) {
	//	cout << "Error! The Point is not in the vector." << endl;
	//}
}

//点在哪一条线上
int getPointPositionInLines(Point point, vector<Line> lines) {
	bool inSegment;
	int index = -1;  //
	for (int i = 0; i < lines.size(); ++i) {
		inSegment = judgeCrossPoint(point, lines[i]);
		if (inSegment) {
			index = i;
			break;
		}
	}
	return index;
}

Line getShoveSegment(const Wire& shoveSingleLines, const Line& slideLine) {
	vector<Line> TempCenterLines = shoveSingleLines.CentralSegments;
	Line SlideSegment = getShoveSegment(TempCenterLines, slideLine);

	return SlideSegment;
}

//找到会被推挤的线段
Line getShoveSegment(vector<Line> shoveSingleLines, Line slideLine) {
	Line outLine;
	if (shoveSingleLines.empty()) {
		return outLine;    // Bug
	}

	double t;
	bool inSlidePath;
	//Line outLine;
	Line parallel_line;
	Point lineVec1, lineVec2;

	lineVec1 = slideLine.Pt2 - slideLine.Pt1;
	lineVec1 = lineVec1 / norm(lineVec1);
	for (int i = 0; i < shoveSingleLines.size(); ++i) {
		parallel_line = shoveSingleLines[i];
		lineVec2 = parallel_line.Pt2 - parallel_line.Pt1;
		lineVec2 = lineVec2 / norm(lineVec2);
		t = cross(lineVec1, lineVec2);

		if (abs(t) < CountMinValue) {
			inSlidePath = determineLineInSlidePath(parallel_line, slideLine);
			if (inSlidePath) {
				outLine = parallel_line;
				break;                    // 11-23 Update
			}
		}
	}

	return outLine;
}

//确定和SlideLine平行的平行线段是否在slide的路径上即会不会被推挤到
bool determineLineInSlidePath(Line parallelLine, Line slideLine) {

	Point parallelLineProjection_1, parallelLineProjection_2;
	Point slideLineProjection_1, slideLineProjection_2;

	parallelLineProjection_1 = getProjectionPoint(parallelLine.Pt1, slideLine);
	parallelLineProjection_2 = getProjectionPoint(parallelLine.Pt2, slideLine);

	slideLineProjection_1 = getProjectionPoint(slideLine.Pt1, parallelLine);
	slideLineProjection_2 = getProjectionPoint(slideLine.Pt2, parallelLine);

	if (judgeCrossPoint(parallelLineProjection_1, slideLine)
		|| judgeCrossPoint(parallelLineProjection_2, slideLine)
		|| judgeCrossPoint(slideLineProjection_1, parallelLine)
		|| judgeCrossPoint(slideLineProjection_2, parallelLine)
		) {
		return true;
	}
	else {
		return false;
	}

}

//交换两个线组
void exchangeTwoLinesVec(vector<Line>& linesLeft, vector<Line>& linesRight) {
	vector<Line> transLinesVec;
	transLinesVec = linesLeft;
	linesLeft = linesRight;
	linesRight = transLinesVec;
}

/*
* @ 线的外扩
* @lineVec： 外扩的线
* @distance: 外扩移动的距离
* @ distance的正负即为外扩的方向  为负：逆时针扩
*/
void linesExpand(vector<Line>& lineVec, double distance) {
	//cout << "Enter in linesExpand......" << endl;
	vector<Line> outLineVec;
	Line tempL;
	Point tempL_vec, tempL_FaXvec;
	Point vec1, vec2;
	Point crossingP;
	double length;

	vector<Line> recordInputLinesVec = lineVec;

	//线段的平移
	for (int i = 0; i < lineVec.size(); ++i) {
		tempL = lineVec[i];
		tempL_vec = tempL.Pt2 - tempL.Pt1;

		tempL_FaXvec = rotate(tempL_vec, -90);  // 顺时针
		length = norm(tempL_FaXvec);
		if (length != 0) {  //非 0 向量
			tempL_FaXvec = tempL_FaXvec / length; //单位化
		}

		lineVec[i].Pt1 = tempL.Pt1 + tempL_FaXvec * distance;
		lineVec[i].Pt2 = tempL.Pt2 + tempL_FaXvec * distance;
	}

	vector<Line> SlidedLinesVec = lineVec;
	//线段的连接  （线段的size > 1）
	for (int i = 0; i + 1 < lineVec.size(); ++i) {
		if (isParallel_lines(SlidedLinesVec[i], SlidedLinesVec[i + 1])) {
			lineVec[i].Pt2 = lineVec[i + 1].Pt1;
		}
		else {
			crossingP = SlidedLinesVec[i].getCrossingPoint(SlidedLinesVec[i + 1]);
			lineVec[i].Pt2 = lineVec[i + 1].Pt1 = crossingP;
		}
	}
	//logData(lineVec, "E:\\PostgraduateProgram\\VsProgram\\Slide\\SlideTest\\outPut\\LogData\\1226SlidePolyLines.txt", true, false);
	// 两端延长
	extendEndingPoint(lineVec, distance);

	//检查连接关系   --- Bug : 12-29 对于交叉的线中间出现多根线段的情况

	//if (lineVec.size() > 2) {
	//	vector<Line> NeedRemovingSegments;
	//	for (int i = 0; i < lineVec.size(); ++i) {
	//		Point oldDir = recordInputLinesVec[i].Pt2 - recordInputLinesVec[i].Pt1;
	//		Point newDir = lineVec[i].Pt2 - lineVec[i].Pt1;
	//		double resultN = oldDir * newDir;
	//		if (resultN < 0) {
	//			NeedRemovingSegments.emplace_back(lineVec[i]);
	//			vec1 = lineVec[i - 1].Pt2 - lineVec[i - 1].Pt1;
	//			vec2 = lineVec[i + 1].Pt2 - lineVec[i + 1].Pt1;
	//			if (isParallel_lines(lineVec[i - 1], lineVec[i + 1])) {
	//				lineVec[i + 1].Pt1 = lineVec[i - 1].Pt2;
	//			}
	//			else {
	//				Point crossP = getInsection(vec1, lineVec[i - 1].Pt2, vec2, lineVec[i + 1].Pt2);
	//				lineVec[i - 1].Pt2 = lineVec[i + 1].Pt1 = crossP;
	//			
	//			}

	//			++i;
	//		}

	//	
	//	}

	//	for (Line& tempRemoveSeg : NeedRemovingSegments) {
	//		removeLine(tempRemoveSeg, lineVec);
	//	}
	//}

	//return;

	int WireSegsSize = lineVec.size();
	if (WireSegsSize > 2) {
		vector<Line> RecoveryLines = lineVec;
		vector<Line> NeedRemovingSegments;
		vector<int> CrossIndex;             // 有交叉的线段的Index
		vector<int> ChangeSegDirSegsIndex;  // 线段走向发生改变的线段Index
		for (int i = 0; i < WireSegsSize - 2; ++i) {
			Line CheckSegment = RecoveryLines[i];  // CheckSegment  // 拿求完交点的原来的线去检查交点（每次发现相交会去处理lineVec，导致原来相交的现在不相交了）
			for (int j = i + 2; j < WireSegsSize; ++j) {
				Line WaitProcessingSeg = lineVec[j];
				// 判断两条线段有交点嘛 有交点则计算交点对二者赋值，并且中间线段全部保存后面删除
				bool CheckR_ = crossSegs(CheckSegment, WaitProcessingSeg);
				if (CheckR_) {
					CrossIndex.emplace_back(j);
				}
			}

			if (!CrossIndex.empty()) {  //非空说明有线段相交
				int lastCrossSegIndex = CrossIndex[CrossIndex.size() - 1];
				Line lastCrossSeg = lineVec[lastCrossSegIndex];
				if (isParallel_lines(CheckSegment, lastCrossSeg)) {
					lineVec[lastCrossSegIndex].Pt1 = lineVec[i].Pt2;
				}
				else {
					Point CrossPoint = CheckSegment.getCrossingPoint(lastCrossSeg);  // 交点
					lineVec[i].Pt2 = lineVec[lastCrossSegIndex].Pt1 = CrossPoint;
				}

				//对中间线段开始记录删除
				for (int k = i + 1; k < lastCrossSegIndex; ++k) {
					NeedRemovingSegments.emplace_back(lineVec[k]);
				}

				if (i != 0) {  // Check Segment Director   
					Point OldSegDir = recordInputLinesVec[i].Pt2 - recordInputLinesVec[i].Pt1;
					Point NewSegDir = lineVec[i].Pt2 - lineVec[i].Pt1;
					int testIndex = getPosition(NeedRemovingSegments, lineVec[i]);
					if ((OldSegDir * NewSegDir) < 0 && testIndex == -1) {    // 线段走向改变且该线段不会被处理（移除）
						ChangeSegDirSegsIndex.emplace_back(i);
					}
				}

				// Loop 
				if (lineVec[lastCrossSegIndex].Pt1 == lineVec[lastCrossSegIndex].Pt2) {
					i = lastCrossSegIndex;
				}
				else {
					i = lastCrossSegIndex - 1;
				}
				CrossIndex.clear();
			}
			else {
				if (i != 0) {  // Check Segment Director   
					Point OldSegDir = recordInputLinesVec[i].Pt2 - recordInputLinesVec[i].Pt1;
					Point NewSegDir = lineVec[i].Pt2 - lineVec[i].Pt1;
					int testIndex = getPosition(NeedRemovingSegments, lineVec[i]);
					if ((OldSegDir * NewSegDir) < 0 && testIndex == -1) {    // 线段走向改变且该线段不会被处理（移除）
						ChangeSegDirSegsIndex.emplace_back(i);
					}
				}
			}

		}

		Point OldSegDir = recordInputLinesVec[WireSegsSize - 2].Pt2 - recordInputLinesVec[WireSegsSize - 2].Pt1;
		Point NewSegDir = lineVec[WireSegsSize - 2].Pt2 - lineVec[WireSegsSize - 2].Pt1;
		int testIndex = getPosition(NeedRemovingSegments, lineVec[WireSegsSize - 2]);
		if ((OldSegDir * NewSegDir) < 0 && testIndex == -1) {    // 线段走向改变且该线段不会被处理（移除）
			ChangeSegDirSegsIndex.emplace_back(WireSegsSize - 2);
		}

		// 对走线方向不对的但是未交叉的线段两端处理下
		for (int i = 0; i < ChangeSegDirSegsIndex.size(); ++i) {
			int DieIndex = ChangeSegDirSegsIndex[i];

			NeedRemovingSegments.emplace_back(lineVec[DieIndex]);  // 走向不正确的要删除掉

			int FirstIndex = DieIndex - 1;
			bool CheckPosition = CheckNumInSets(FirstIndex, ChangeSegDirSegsIndex);  // 有点多余
			while (CheckPosition) {
				FirstIndex--;
				CheckPosition = CheckNumInSets(FirstIndex, ChangeSegDirSegsIndex);
			}

			int BackIndex = DieIndex + 1;
			CheckPosition = CheckNumInSets(BackIndex, ChangeSegDirSegsIndex);  // 判断下一条线段是不是也是走向有问题的
			while (CheckPosition) {
				NeedRemovingSegments.emplace_back(lineVec[BackIndex]);  // 走向不正确的要删除掉
				BackIndex++;
				++i;
				CheckPosition = CheckNumInSets(BackIndex, ChangeSegDirSegsIndex);
			}

			Line FirstSeg = lineVec[FirstIndex];
			Line BackSeg = lineVec[BackIndex];
			if (abs(cross((FirstSeg.Pt2 - FirstSeg.Pt1), (BackSeg.Pt2 - BackSeg.Pt1))) < CountMinValue) {  // 两端的线是平行的
				lineVec[FirstIndex].Pt2 = lineVec[BackIndex].Pt2;
				NeedRemovingSegments.emplace_back(lineVec[BackIndex]);
			}
			else { // 非平行 算交点
				Point CrossPoint = FirstSeg.getCrossingPoint(BackSeg);  // 交点
				lineVec[FirstIndex].Pt2 = lineVec[BackIndex].Pt1 = CrossPoint;
			}
		}

		// 删除绕线交叉的线段 && 删除走线不对的但是未交叉的线段
		for (Line& tempRemoveSeg : NeedRemovingSegments) {
			removeLine(tempRemoveSeg, lineVec);
		}

	}
	//logData(lineVec, "E:\\PostgraduateProgram\\VsProgram\\Slide\\SlideTest\\outPut\\LogData\\1226SlidePolyLines.txt", false, false);
	inspectSegmentsConnection(lineVec);

}

//一根线两端点的延长
void extendEndingPoint(vector<Line>& singleLinesVec, const double& dist) {
	Line tempEdgeSegment = singleLinesVec[0];
	if (singleLinesVec.size() > 1) {  //多条线段的问题
		if (tempEdgeSegment.Pt2 == singleLinesVec[1].Pt1) {
			//起始
			Point expandVec = tempEdgeSegment.Pt1 - tempEdgeSegment.Pt2;
			expandVec = expandVec / norm(expandVec);
			//expandVec.normalizeVec();
			tempEdgeSegment.Pt1 = tempEdgeSegment.Pt1 + expandVec * abs(dist);
			singleLinesVec[0] = tempEdgeSegment;

			//终止
			Line endingSegment = singleLinesVec[singleLinesVec.size() - 1];
			expandVec = endingSegment.Pt2 - endingSegment.Pt1;
			expandVec = expandVec / norm(expandVec);
			//expandVec.normalizeVec();
			endingSegment.Pt2 = endingSegment.Pt2 + expandVec * abs(dist);
			singleLinesVec[singleLinesVec.size() - 1] = endingSegment;

		}
		else {
			//起始
			Point expandVec = tempEdgeSegment.Pt2 - tempEdgeSegment.Pt1;
			expandVec = expandVec / norm(expandVec);
			//expandVec.normalizeVec();
			tempEdgeSegment.Pt2 = tempEdgeSegment.Pt2 + expandVec * abs(dist);
			singleLinesVec[0] = tempEdgeSegment;
			//终止
			Line endingSegment = singleLinesVec[singleLinesVec.size() - 1];
			expandVec = endingSegment.Pt1 - endingSegment.Pt2;
			//expandVec.normalizeVec();
			expandVec = expandVec / norm(expandVec);
			endingSegment.Pt1 = endingSegment.Pt1 + expandVec * abs(dist);
			singleLinesVec[singleLinesVec.size() - 1] = endingSegment;
		}
	}
	else {  //单条线段
		Point tempExpandVec = tempEdgeSegment.Pt1 - tempEdgeSegment.Pt2;
		tempExpandVec = tempExpandVec / norm(tempExpandVec);
		tempEdgeSegment.Pt1 = tempEdgeSegment.Pt1 + tempExpandVec * abs(dist);
		tempEdgeSegment.Pt2 = tempEdgeSegment.Pt2 + (tempExpandVec)*abs(dist) * (-1);
		singleLinesVec[0] = tempEdgeSegment;
	}
}


//对整根线的检查 —— 对相邻平行的线段进行合并
void inspectSegmentsConnection(vector<Line>& singleLinesVec) {
	int numSizeCount = singleLinesVec.size();
	if (numSizeCount <= 1) {
		return;
	}

	CheckWireLinesCross(singleLinesVec); // Check Cross

	sortSingleLines(singleLinesVec);   // Check order

	DeleteSameSegment(singleLinesVec);   // 删除相同的相邻线段

	numSizeCount = singleLinesVec.size(); // 12-20 Update
	vector<Line> NeedRemoveSegments;
	for (int i = 0; i + 1 < numSizeCount; ++i) {
		Line tempCurrentSegment_ = singleLinesVec[i];
		Point baseVec = tempCurrentSegment_.Pt2 - tempCurrentSegment_.Pt1;
		baseVec = baseVec / norm(baseVec);
		for (int j = i + 1; j < numSizeCount; ++j) {
			Line tempNextSegment_ = singleLinesVec[j];
			Point nextVec = tempNextSegment_.Pt2 - tempNextSegment_.Pt1;
			nextVec = nextVec / norm(nextVec);
			double t = cross(baseVec, nextVec);
			if (abs(t) < CountMinValue) {   // "//"  相邻线平行即共线 (小于 0.1°判为共线)
				NeedRemoveSegments.emplace_back(tempNextSegment_);
				singleLinesVec[i].setPt2(tempNextSegment_.Pt2);

				if (j != numSizeCount - 1) {
					continue;
				}
				else {
					i = j;  // 最后一个线段也被优化完就退出 -- 12-25 Update
					break;
				}
			}
			else {             // 非平行线
				i = j - 1;     // 往后迭代
				break;
			}
		}
	}

	// 去除有平行关系的线
	for (Line& itemSeg : NeedRemoveSegments) {
		removeLine(itemSeg, singleLinesVec);
	}

}

// 对整根线进行排序
void sortSingleLines(vector<Line>& singleLinesVec) {
	int numSizeCount = singleLinesVec.size();
	if (numSizeCount <= 1) {
		return;
	}

	RemovePointSegment(singleLinesVec);  // 12-20 Update
	//Point endingPoint;
	Line firstSegment;

	for (Line& itemSeg : singleLinesVec) {
		bool isEndingPoint = true;
		// 检查Pt1
		Point sidePoint = itemSeg.Pt1;
		for (auto& tempSegments : singleLinesVec) {
			if (tempSegments == itemSeg) {
				continue;
			}
			else {
				if (tempSegments.Pt2 == sidePoint || tempSegments.Pt1 == sidePoint) {
					isEndingPoint = false;  //说明SidePoint不是端点
					break;
				}
			}
		}

		if (isEndingPoint) {  // 说明这个Pt1就是
			//endingPoint = sidePoint;
			firstSegment = itemSeg;
			break;
		}

		// 检查Pt2
		isEndingPoint = true;
		sidePoint = itemSeg.Pt2;
		for (auto& tempSegments : singleLinesVec) {
			if (tempSegments == itemSeg) {
				continue;
			}
			else {
				if (tempSegments.Pt2 == sidePoint || tempSegments.Pt1 == sidePoint) {
					isEndingPoint = false;  //说明SidePoint不是端点
					break;
				}
			}
		}

		if (isEndingPoint) {  // 说明这个Pt2就是
			//endingPoint = sidePoint;
			exchangePoints(itemSeg.Pt1, itemSeg.Pt2);
			firstSegment = itemSeg;
			break;
		}
	}

	// 找到第一条线段后 循环找到Sideline
	vector<Line> outLines;
	outLines.emplace_back(firstSegment);
	removeLine(firstSegment, singleLinesVec);
	int newSizeVec = singleLinesVec.size();
	for (int i = 0; i < newSizeVec; ++i) {
		Line nextLine = getSideLine(firstSegment, singleLinesVec);
		removeLine(nextLine, singleLinesVec);
		firstSegment = nextLine;
		outLines.emplace_back(firstSegment);
	}

	singleLinesVec.clear();  //清空
	for (int j = 0; j < outLines.size(); ++j) {
		singleLinesVec.emplace_back(outLines[j]);
	}
}

// 去除线组中起点和终点相同的线段
void RemovePointSegment(vector<Line>& InputCheckSegs) {
	if (InputCheckSegs.empty()) {
		return;
	}
	vector<Line>ReadBackUpSegs = InputCheckSegs;
	for (auto& itemSeg : ReadBackUpSegs) {
		if (itemSeg.Pt1 == itemSeg.Pt2) {
			removeLine(itemSeg, InputCheckSegs);
		}
	}
}

//判断两根线的交点
bool intersectionTwoLines(vector<Line> lines_1, vector<Line> lines_2) {
	bool intersectionPoints = false;
	bool checkCrossingPoint;   //检查交点合理否

	vector<Point> tempIntersectionPoints;
	vector<Point> twoLinesEndPoints;   //两条线所有的交点
	Line tempLine;

	//Init
	twoLinesEndPoints = LinesToPoints(lines_1, false);
	tempIntersectionPoints = LinesToPoints(lines_2, false);
	for (int k = 0; k < tempIntersectionPoints.size(); ++k) {
		twoLinesEndPoints.emplace_back(tempIntersectionPoints[k]);
	}
	tempIntersectionPoints.clear();

	//Find crossingPoints
	for (int i = 0; i < lines_1.size(); ++i) {
		tempLine = lines_1[i];
		tempIntersectionPoints = insectionsOfLineToLines(tempLine, lines_2);
		//判断交点是否全是端点
		checkCrossingPoint = checkSamePointsVec(twoLinesEndPoints, tempIntersectionPoints);
		if (!tempIntersectionPoints.empty() && !checkCrossingPoint) {

			intersectionPoints = true;
			break;
		}
	}

	return intersectionPoints;
}

/*
* @brief: 求一条线段和多条线段的交点
* @line: 一条线段
* @lines: 线组
* @输出： Points即为交点
*/
vector<Point> insectionsOfLineToLines(Line line, vector<Line> lines) {
	bool tempJudge;
	bool inPointsVec;
	Point pTemp;
	Point vec1, vec2;
	vector<Point> outPoints;
	double dotResult;
	std::unordered_set<Point, PointHash> tmpCrossPoints;

	vec1 = line.Pt2 - line.Pt1;
	for (int i = 0; i < lines.size(); ++i) {

		vec2 = lines[i].Pt2 - lines[i].Pt1;
		NormalizeVec(vec1);
		NormalizeVec(vec2);
		dotResult = cross(vec1, vec2);
		if (abs(dotResult) < CountMinValue) {  // 是"//"线
			continue;
		}

		//tempJudge = crossSegs(line, lines[i]);
		tempJudge = CheckSegsCrossBoost(line, lines[i]);
		if (tempJudge) {
			pTemp = getInsection(vec1, line.Pt1, vec2, lines[i].Pt1);

			if (tmpCrossPoints.count(pTemp) <= 0) {
				tmpCrossPoints.insert(pTemp);
				outPoints.emplace_back(pTemp);
			}

			//inPointsVec = pointInPointsVec(pTemp, outPoints);
			//if (!inPointsVec) {
			//	outPoints.emplace_back(pTemp);
			//}
		}
	}

	if (outPoints.size() > 1) {
		sortPoints(line, outPoints);  //排序的过程
	}

	return outPoints;
}

//将最后的点从起点到终点依次排好
void sortPoints(Line line, vector<Point>& points) {
	Point startP = line.Pt1;  //起点
	Point tempPoint;
	double dist1, dist2;

	if (points.empty()) {
		return;
	}

	for (int i = 0; i < points.size() - 1; ++i) {
		dist1 = PointsDist(points[i], startP);
		for (int j = i + 1; j < points.size(); ++j) {
			dist2 = PointsDist(points[j], startP);
			if (dist2 < dist1) {  //points[j]离得近  -> 两点交换
				tempPoint = points[i];
				points[i] = points[j];
				points[j] = tempPoint;
				dist1 = dist2;
			}
		}
	}
}

// 计算两个点之间的曼哈顿距离
double PointsDist(Point p1, Point p2) {
	double distance;
	distance = (abs(p1.x - p2.x) + abs(p1.y - p2.y));
	return distance;
}

/*
* @ //判断pointsVec_small中是不是只有顶点没有其他点了
* @ pointsVec_large：端点的集合
* @ pointsVec_small：测试的交点集合
* @ || 主要是判断pointsVec_small中的点是不是都在pointsVec_large里面：return true: 都是端点 ; return false： 有别的交点
*/
bool checkSamePointsVec(const vector<Point>& pointsVec_large, const vector<Point>& pointsVec_small) {
	bool checkInLarge = true;   // 默认所有点都在

	for (int i = 0; i < pointsVec_small.size(); ++i) {
		checkInLarge = pointInPointsVec(pointsVec_small[i], pointsVec_large);
		if (!checkInLarge) {
			break;
		}
	}
	return checkInLarge;
}


//交换两条线段
void exchangeLine(Line& line1, Line& line2) {
	Line tempMidLine;
	tempMidLine = line1;
	line1 = line2;
	line2 = tempMidLine;
}

void exchangeTwoNum(int& num1, int& num2) {
	int tempMidNum;
	tempMidNum = num1;
	num1 = num2;
	num2 = tempMidNum;
}

/*//判断线段与多边形是否相交
* limitCrossing 用来判断是否考虑端点相交的情况 —— true： 考虑线段端点为交点的情况
*/
bool isSegPolygonCross(const Line& inputLine, const vector<Line>& polygon, const bool& limitCrossing) {
	Line line = inputLine;
	//if (inPolygon(line.Pt1, polygon) || inPolygon(line.Pt2, polygon) || inPolygon((line.Pt2 + line.Pt1) / 2, polygon)) {
	//	return true;
	//}

	vector<Point> crossingPoints = insectionsOfLineToLines(line, polygon);

	if (!limitCrossing) {   // 不考虑端点相交
		if (pointInPointsVec(line.Pt1, crossingPoints)) {
			removePoint(line.Pt1, crossingPoints);
		}

		if (pointInPointsVec(line.Pt2, crossingPoints)) {
			removePoint(line.Pt2, crossingPoints);
		}
	}

	if (crossingPoints.empty()) {
		if (pointInPolygon((line.Pt2 + line.Pt1) / 2, { polygon }, false)) {
			return true;
		}
		else {
			return false;
		}
	}
	else {
		return true;
	}

	return false;
}


//计算一组线段的总长度  // 2023/11/9 update
double countSegmentsLength(const vector<Line>& lines) {
	double totalLength = 0;
	if (lines.empty()) {
		return DoubleMaxNumValue;  // 没有线的话确保不取这条线  //11-16 Update
	}

	for (int i = 0; i < lines.size(); ++i) {
		Line tempSegment = lines[i];
		double tempLength = tempSegment.getLength();
		totalLength = totalLength + tempLength;
	}

	return totalLength;
}


/*
* @//余弦定理求余弦值
* @oppoSide：对边
* @nextLeftSide || nextRightSide：两个邻边
*/
double getCosineValue(double oppoSide, double nextLeftSide, double nextRightSide) {
	double numerator, denominator;
	numerator = nextLeftSide * nextLeftSide + nextRightSide * nextRightSide - oppoSide * oppoSide;
	denominator = 2 * nextLeftSide * nextRightSide;
	return ((numerator) / (denominator) > 1.0) ? 1.0 : (numerator) / (denominator);
}

vector<Line> WireExpandBoost(const Wire& InputWireSegs, const double& ExpandDistance) {
	vector<Line> CenterInputLine = InputWireSegs.CentralSegments;
	double buffer_distance = (InputWireSegs.WireWidth + ExpandDistance) * PCIBoost;
	const int points_per_circle = 16;
	typedef double coordinate_type;
	typedef boost::geometry::model::d2::point_xy<coordinate_type> point;
	typedef boost::geometry::model::polygon<point> polygon;
	bg::strategy::buffer::distance_symmetric<coordinate_type> distance_strategy(buffer_distance);
	//bg::strategy::buffer::join_round join_strategy(points_per_circle);
	//bg::strategy::buffer::end_round end_strategy(points_per_circle);
	bg::strategy::buffer::join_miter join_strategy;  // 直线连接拐角
	bg::strategy::buffer::end_flat end_strategy;
	bg::strategy::buffer::point_circle circle_strategy(points_per_circle);
	bg::strategy::buffer::side_straight side_strategy;

	// Declare output
	bg::model::multi_polygon<polygon> result;
	// Declare/fill a linestring
	boost::geometry::model::linestring<point> ls;
	vector<Point> WirePoints = LinesToPoints(CenterInputLine, false);
	for (int i = 0; i < WirePoints.size(); ++i) {
		Point tempPoint = WirePoints[i];
		point ExchangeP(tempPoint.x * PCIBoost, tempPoint.y * PCIBoost);
		/*    point ExchangeP;
			  ExchangeP.set<0>(tempPoint.x);
			  ExchangeP.set<1>(tempPoint.y);   */
		ls.emplace_back(ExchangeP);
	}


	// Create the buffer of a linestring
	boost::geometry::buffer(ls, result,
		distance_strategy, side_strategy,
		join_strategy, end_strategy, circle_strategy);


	// 输出多边形的坐标
	vector<Point> OuterPoints;
	vector<Line> OuterPolygon;
	vector<vector<Line> >OutPolygonSegs;
	for (int i = 0; i < result.size(); ++i) {
		polygon tempPolygon = result[i];
		for (const auto& pt : tempPolygon.outer())
		{
			// 在这里处理每个点，例如输出坐标
			double x = bg::get<0>(pt);
			double y = bg::get<1>(pt);
			Point tempOutP(x / PCIBoost, y / PCIBoost);
			OuterPoints.emplace_back(tempOutP);

		}
		OuterPolygon = PointsToLines(OuterPoints, true);
		OutPolygonSegs.emplace_back(OuterPolygon);
		OuterPoints.clear();
		OuterPolygon.clear();
	}

	if (OutPolygonSegs.empty()) { // Expand Wrong! 2024-03-21 Ge Update

		return OuterPolygon;
	}
	else {
		return OutPolygonSegs[0];
	}
}

double getParallelLineDist(const Line& OneSeg, const Line& SecondSeg) {

	return  pointToLineDist(OneSeg.Pt1, SecondSeg);
}

// 同步师兄的代码 --- 无需合并 2024-01-08
bool BOOSTinPolygon(const Point& pt, const std::vector<Line>& poly) {
	typedef boost::geometry::model::d2::point_xy<double> point_t;
	typedef boost::geometry::model::polygon<point_t> polygon_t;

	vector<Point> pts = LinesToPoints(poly, true);

	if (pts.front() != pts.back())
		pts.emplace_back(pts.front());
	if (!isClockwise(pts))
		reverse(pts.begin(), pts.end());
	polygon_t polygon;
	for (const auto& p : pts) {
		boost::geometry::append(polygon.outer(), point_t(p.x, p.y));
	}

	return boost::geometry::within(point_t(pt.x, pt.y), polygon);
}

// 同步师兄的代码 --- 无需合并 2024-01-08
bool isClockwise(const std::vector<Point>& points) {
	double d = 0;
	for (int i = 0; i < points.size(); ++i) {
		d += points[i].x * points[(i + 1) % points.size()].y - points[(i + 1) % points.size()].x * points[i].y;
	}

	if (d > 0)
		return false;
	else
		return true;

}

// ConsiderEndPoints = true : lines_1端点为交点的时候需要返回这个交点
vector<Point> getTwoLinesCrossingPoints(const vector<Line>& lines_1, const vector<Line>& lines_2, const bool& ConsiderEndPoints) {

	vector<Point> tempIntersectionPoints;  // 保存临时交点
	vector<Point> twoLinesEndPoints;   // 两条线所有的端点
	Line tempLine;

	if (lines_1.empty() || lines_2.empty()) {
		return tempIntersectionPoints;
	}

	//Init
	twoLinesEndPoints = LinesToPoints(lines_1, false);
	tempIntersectionPoints = LinesToPoints(lines_2, false);
	for (int k = 0; k < tempIntersectionPoints.size(); ++k) {
		twoLinesEndPoints.emplace_back(tempIntersectionPoints[k]);
	}
	tempIntersectionPoints.clear();

	vector<Point> TotalPoints;
	//Find crossingPoints
	for (int i = 0; i < lines_1.size(); ++i) {
		tempLine = lines_1[i];
		tempIntersectionPoints = insectionsOfLineToLines(tempLine, lines_2);

		if (!ConsiderEndPoints) {  // 不考虑端点(线的起点和终点)的话
			if (pointInPointsVec(lines_1[0].Pt1, tempIntersectionPoints)) {
				removePoint(lines_1[0].Pt1, tempIntersectionPoints);
			}
			if (pointInPointsVec(lines_1[lines_1.size() - 1].Pt2, tempIntersectionPoints)) {
				removePoint(lines_1[lines_1.size() - 1].Pt2, tempIntersectionPoints);
			}
		}

		if (!tempIntersectionPoints.empty()) {
			for (auto& itemPoint : tempIntersectionPoints) {
				if (!pointInPointsVec(itemPoint, TotalPoints)) {
					TotalPoints.emplace_back(itemPoint);
				}
			}
		}

		tempIntersectionPoints.clear();

	}

	return TotalPoints;

}

//  *** StartP_&& EndP_都在InputWireLines上
bool LiveWireGetNewSegment(const Point& StartP_, const Point& EndP_, const vector<Line>& InputWireLines, vector<Line>& PrintOutHugLines) {

	//Sort InputWireLines
	vector<Line> BackInputLines = InputWireLines;
	sortSingleLines(BackInputLines);

	int StartPointPositionIndex = getPointPositionInLines(StartP_, BackInputLines);
	int EndPointPositionIndex = getPointPositionInLines(EndP_, BackInputLines);
	if (StartPointPositionIndex == -1 || EndPointPositionIndex == -1) {
		return false;
	}

	if (StartPointPositionIndex <= EndPointPositionIndex) {
		for (int i = 0; i < BackInputLines.size(); ++i) {
			if (i >= StartPointPositionIndex && i <= EndPointPositionIndex) {
				PrintOutHugLines.emplace_back(BackInputLines[i]);
			}
		}
	}
	else {
		for (int i = 0; i < BackInputLines.size(); ++i) {
			if (i >= EndPointPositionIndex && i <= StartPointPositionIndex) {
				PrintOutHugLines.emplace_back(BackInputLines[i]);
			}
		}
	}
	if (PrintOutHugLines.empty()) {
		return false;
	}
	else {
		//头尾的处理  PrintOutHugLines 一定不为空
		PrintOutHugLines[0].setPt1(StartP_);
		PrintOutHugLines[PrintOutHugLines.size() - 1].setPt2(EndP_);
	}

	return true;
}

//将本次推挤可能需要Hug的线添加进来  --- index 合规才可以调进来

// ConsiderEndPoints == true : 考虑交点为InputSegment端点的情况  ---- 未启用
bool getSegmentsToLinesCrossingPoints(const Line& InputSegment, const vector<vector<Line> >& InputLinesVec, const bool& ConsiderEndPoints) {
	vector<Point> CrossingPoints;

	if (InputLinesVec.empty()) {
		return false;
	}

	for (int i = 0; i < InputLinesVec.size(); ++i) {
		vector<Point> TempPointsC = insectionsOfLineToLines(InputSegment, InputLinesVec[i]);
		if (TempPointsC.size() % 2 != 0) {			// 有障碍物的交点是奇数
			return false;
		}

	}

	return true;
}

//Return the line segments in line group a that are not in line group b
bool RetrunTrueHugSegments(const vector<Line>& PreSlideWires, const vector<Line>& AfterSlideWires, vector<Line>& RealHugSegments) {
	if (PreSlideWires.empty() || AfterSlideWires.empty()) {
		return false;
	}

	// 遍历AfterSlideWires中的每一条线段：如果不在PreSlideWires中，说明这次推挤影响了这条线段（需要参与Hug的Segment）
	Line TempSeg;
	for (int i = 0; i < AfterSlideWires.size(); ++i) {
		TempSeg = AfterSlideWires[i];
		if (getPosition(PreSlideWires, TempSeg) == -1) {
			RealHugSegments.emplace_back(TempSeg);
		}
	}

	if (RealHugSegments.empty()) {
		return false;
	}
	else {
		return true;
	}
}


bool linesExpandToPoly(const vector<Line>& linesVec, double distance, vector<Line>& ExpandResultPoly) {
	if (linesVec.empty()) {
		return false;
	}

	vector<Line> front = linesVec;
	vector<Line> back = linesVec;
	linesExpand(front, distance);
	linesExpand(back, -distance);

	addPoint(front, back.back().getPt2());
	reverse(back.begin(), back.end());
	for (Line& l : back) {
		front.emplace_back(lineReverse(l));
	}

	addPoint(front, front.front().getPt1());

	ExpandResultPoly = front;
	return true;
}

void addPoint(vector<Line>& line, Point pt) {
	if (line.empty()) {
		return;
	}

	line.emplace_back(Line(line.back().getPt2(), pt));
}

Line lineReverse(Line l) {

	return Line(l.getPt2(), l.getPt1());
}

bool isInWhichParallelSegs(const Line& InputSlideSeg, const vector<Line>& InputParallelSegs, const vector<int>& NumIndex) {
	if (InputParallelSegs.empty()) {
		return false;
	}

	if (!NumIndex.empty()) {
		for (auto& TempIndex : NumIndex) {
			if (isParallel_lines(InputSlideSeg, InputParallelSegs[TempIndex])) {
				return true;
			}
		}
	}
	else {
		for (auto& TempSegment : InputParallelSegs) {
			if (isParallel_lines(InputSlideSeg, TempSegment)) {
				return true;
			}
		}

	}

	return false;
}

// 使用Boost库计算两条线段的交点
bool BoostGetCrossingPoints(const Point& InputPointOne, const Point& InputVecOne, const Point& InputPointTwo, const Point& InputVecTwo, Point& CrossingPoint) {
	using PointBoost = bg::model::d2::point_xy<double>;
	using Segment = bg::model::segment<PointBoost>;

	PointBoost point1(InputPointOne.x, InputPointOne.y);
	PointBoost direction1(InputVecOne.x + InputPointOne.x, InputVecOne.y + InputPointOne.y);

	PointBoost point2(InputPointTwo.x, InputPointTwo.y);
	PointBoost direction2(InputVecTwo.x + InputPointTwo.x, InputVecTwo.y + InputPointTwo.y);
	Segment SegmentOne(point1, direction1);
	Segment SegmentTwo(point2, direction2);
	// 计算两条线段的延长线
	Segment extendedLine1 = bg::return_envelope<Segment>(SegmentOne);
	Segment extendedLine2 = bg::return_envelope<Segment>(SegmentTwo);
	std::vector<PointBoost> intersectionPoints;
	// 使用 boost::geometry::intersection 函数计算交点
	bool hasIntersection = bg::intersection(extendedLine1, extendedLine2, intersectionPoints);
	if (hasIntersection && !intersectionPoints.empty()) {
		CrossingPoint = Point(bg::get<0>(intersectionPoints[0]), bg::get<1>(intersectionPoints[0]));
		return true;
	}
	else {
		return false;
	}
}

bool BoostGetCrossingPoints(const Line& InputSegOne, const Line& InputSegTwo, Point& CrossingPoint) {
	if (isParallel_lines(InputSegOne, InputSegTwo)) {  // "//" --- Wrong!
		return false;
	}

	using PointBoost = bg::model::d2::point_xy<double>;
	using Segment = bg::model::segment<PointBoost>;
	PointBoost pointOneS(InputSegOne.Pt1.x, InputSegOne.Pt1.y);
	PointBoost pointOneE(InputSegOne.Pt2.x, InputSegOne.Pt2.y);

	PointBoost pointTwoS(InputSegTwo.Pt1.x, InputSegTwo.Pt1.y);
	PointBoost pointTwoE(InputSegTwo.Pt2.x, InputSegTwo.Pt2.y);
	Segment SegmentOne(pointOneS, pointOneE);
	Segment SegmentTwo(pointTwoS, pointTwoE);

	std::vector<PointBoost> intersectionPoints;
	// 使用 boost::geometry::intersection 函数计算交点
	bool hasIntersection = bg::intersection(SegmentOne, SegmentTwo, intersectionPoints);
	if (hasIntersection && !intersectionPoints.empty()) {
		CrossingPoint = Point(bg::get<0>(intersectionPoints[0]), bg::get<1>(intersectionPoints[0]));
		return true;
	}
	else {
		return false;
	}

}

bool CheckWireLinesCross(vector<Line>& InputSingleLinesVec) {
	if (InputSingleLinesVec.size() < 3) {
		return false;
	}

	Line BaseCheckSgement, ProcessLoopCheckSeg;
	bool CheckCrossRes = false;
	vector<Line> NeedDeleteSegs;
	for (int i = 0; i < InputSingleLinesVec.size() - 2; ++i) {
		BaseCheckSgement = InputSingleLinesVec[i];
		for (int j = i + 2; j < InputSingleLinesVec.size(); ++j) {
			ProcessLoopCheckSeg = InputSingleLinesVec[j];
			CheckCrossRes = crossSegs(BaseCheckSgement, ProcessLoopCheckSeg);
			if (CheckCrossRes) { // 线内交叉
				CheckCrossRes = false;
				Point CrossingPoint;
				if (BoostGetCrossingPoints(BaseCheckSgement, ProcessLoopCheckSeg, CrossingPoint)) {
					InputSingleLinesVec[i].Pt2 = InputSingleLinesVec[j].Pt1 = CrossingPoint;
					for (int k = i + 1; k < j; ++k) {
						NeedDeleteSegs.emplace_back(InputSingleLinesVec[k]);
					}
					i = j; // Loop
					break;
				}
			}
		}
	}

	//Delete Segs
	for (Line& tempRemoveSeg : NeedDeleteSegs) {
		removeLine(tempRemoveSeg, InputSingleLinesVec);
	}

	return true;
}


// 点到一组线段中的哪条线段最近
int GetPointToSideMinDist(const Point& InputPoint, const vector<Line>& PolySegments) {
	int IndexSeg = -1;
	double MinDist = DoubleMaxNumValue;
	double CurDist = DoubleMaxNumValue;
	for (int i = 0; i < PolySegments.size(); ++i) {
		CurDist = pointToSegmentDistance(InputPoint, PolySegments[i]);
		if (CurDist < MinDist) {
			IndexSeg = i;
			MinDist = CurDist;
		}
	}

	return IndexSeg;
}

/*
* @ brief : //// Hug中对Bus按SlideDir进行排序 从SlideDir方向从前向后
* @ inputId : 一组线的Id Output
* @ BaseSegs: 判断的基准线段组
* @ SlideDir： Slide的方向
* @ SortDir: true——>从SlideDir方向从前向后(箭头到尾部 )  false : 从后指向箭头
*/
bool sortBusSlideDir(vector<int>& inputId, const vector<Line>& BaseSegs, const Point& SlideDir, const bool& SortDir) {
	if (inputId.empty() || (inputId.size() != BaseSegs.size())) {
		return false;
	}
	int numSizeID = inputId.size();
	for (int i = 0; i < numSizeID - 1; ++i) {
		for (int j = 0; j < numSizeID - i - 1; ++j) {
			int baseNum = inputId[j];
			int nextNum = inputId[j + 1];
			Line LineFront_ = BaseSegs[baseNum];
			Line LineBack_ = BaseSegs[nextNum];
			Point tempVecDir;
			if (SortDir) {
				tempVecDir = getMidpoint(LineFront_) - getMidpoint(LineBack_);   // ② ——> ①
			}
			else {
				tempVecDir = getMidpoint(LineBack_) - getMidpoint(LineFront_);   // 1 ——> 2
			}

			tempVecDir = getSlideProjectionUtils(tempVecDir, rotate((LineBack_.Pt2 - LineBack_.Pt1), 90));
			if (!inspectVectorsDirection(tempVecDir, SlideDir)) {
				exchangeTwoNum(inputId[j], inputId[j + 1]);    // Exchange Index.
			}

		}
	}
	return true;
}

//求解offsetVec在baseVec方向上的投影方向
Point getSlideProjectionUtils(const Point& offsetVec, const Point& baseVec) {
	Point newBaseVec = baseVec;
	Point newOffsetVec = offsetVec;
	newBaseVec = newBaseVec / norm(newBaseVec);
	if (newOffsetVec != Point(0, 0)) {
		NormalizeVec(newOffsetVec);  // 单位化 -- 2024-01-11 Update
	}
	Point projectionVec;
	double isSameDir = newOffsetVec * newBaseVec;   // 判断两个向量方向是否相同（用单位向量）
	if (abs(isSameDir) < CountMinValue) {
		isSameDir = 0;
	}
	if (isSameDir >= 0) {    // Same
		projectionVec = newBaseVec * isSameDir;
	}
	else {
		newBaseVec = newBaseVec * (-1);
		double newDotValue = newOffsetVec * newBaseVec;
		projectionVec = newBaseVec * newDotValue;
	}

	return projectionVec;
}

void reversePolygon(std::vector<Line>& poly) {
	auto pts = LinesToPoints(poly, true);
	reverse(pts.begin(), pts.end());
	poly = PointsToLines(pts, true);
}

// 计算一条线段和一个多边形的交点
bool getSegAndPolyInsectionsBoost
(const Line& inputSegment,
	const std::vector<Line>& inputPolygon,
	std::vector<Point>& CrossingPoints)
{
	if (inputPolygon.empty()) {
		return false;
	}

	typedef bg::model::d2::point_xy<double> point_type;
	typedef bg::model::segment<point_type> segment_type;
	typedef bg::model::polygon<point_type> polygon_type;

	// 定义线段和多边形
	segment_type segment(point_type(inputSegment.Pt1.x, inputSegment.Pt1.y), point_type(inputSegment.Pt2.x, inputSegment.Pt2.y));

	polygon_type polygon;
	vector<Point> inputPoints = LinesToPoints(inputPolygon, true);
	for (auto& itemSeg : inputPoints) {
		bg::append(polygon, point_type(itemSeg.x, itemSeg.y));
	}

	// 计算交点
	std::vector<point_type> intersection_points;
	//bg::intersection(segment, polygon, intersection_points);
	bool intersects = boost::geometry::intersects(segment, polygon);

	// 输出交点
	if (intersection_points.empty()) {
		return false;
	}
	else {
		for (auto& itemPoint : intersection_points) {
			Point temPoint(itemPoint.x(), itemPoint.y());
			CrossingPoints.emplace_back(temPoint);
		}

		return true;
	}

}

bool CheckSegsCrossBoost(const Line& SegOne, const Line& SegTwo) {
	typedef boost::geometry::model::point<double, 2, boost::geometry::cs::cartesian> point_type;
	typedef boost::geometry::model::segment<point_type> segment_type;

	// Define two line segments
	segment_type segment1(point_type(SegOne.Pt1.x, SegOne.Pt1.y), point_type(SegOne.Pt2.x, SegOne.Pt2.y));
	segment_type segment2(point_type(SegTwo.Pt1.x, SegTwo.Pt1.y), point_type(SegTwo.Pt2.x, SegTwo.Pt2.y));

	// Check if the segments intersect
	bool intersects = boost::geometry::intersects(segment1, segment2);

	return intersects;
}

bool CheckSegCrossPBoost(const Line& inputSegment, const std::vector<Line>& inputPolygon) {
	if (inputPolygon.empty()) {
		return false;
	}

	typedef bg::model::d2::point_xy<double> point_type;
	typedef bg::model::segment<point_type> segment_type;
	typedef bg::model::polygon<point_type> polygon_type;

	// 定义线段和多边形
	segment_type segment(point_type(inputSegment.Pt1.x, inputSegment.Pt1.y), point_type(inputSegment.Pt2.x, inputSegment.Pt2.y));

	polygon_type polygon;
	vector<Point> inputPoints = LinesToPoints(inputPolygon, true);
	for (auto& itemSeg : inputPoints) {
		bg::append(polygon, point_type(itemSeg.x, itemSeg.y));
	}

	// 计算交点
	std::vector<point_type> intersection_points;
	bool intersects = boost::geometry::intersects(segment, polygon);
	if (intersects) {
		return true;
	}
	else {
		return false;
	}

}

// inputPathOne 的线段是否存在inputPathTwo中 return tru---> In
bool CheckRoutePathSame(const vector<Line>& inputPathOne, const vector<Line>& inputPathTwo) {
	if (inputPathOne.empty() || inputPathTwo.empty()) {
		return false;
	}

	vector<Point> PathOne = LinesToPoints(inputPathOne, false);
	std::unordered_set<Point, PointHash> PathTwo;
	PathTwo.insert(inputPathTwo[0].Pt1);
	for (int i = 0; i < inputPathTwo.size(); ++i) {
		PathTwo.insert(inputPathTwo[i].Pt2);
	}

	Point checkPoint = PathOne[1];

	if (PathTwo.count(checkPoint) > 0) {
		return true;
	}
	else {
		return false;
	}
}

